#include "CollisionMesh.h"
#include "Mgc/MgcDistLin3Tri3.h"
#include <windows.h>
#include "Plane.h"

#include <GL/gl.h>

void CollisionMesh::AddTri(const Tri& t)
{
	m_tris.push_back(t);
}

void CollisionMesh::Draw()
{
	glBegin(GL_TRIANGLES);
	for (unsigned int i = 0; i < m_tris.size(); i++)
	{
		const Tri& tri = m_tris[i];
		glNormal3f(tri.m_normal.m_x, tri.m_normal.m_y, tri.m_normal.m_z);
		for (int j = 0; j < 3; j++)
		{
			glVertex3f(tri.m_verts[j].x(), tri.m_verts[j].y(), tri.m_verts[j].z());
		}
	}   
	glEnd();
}

// Calculate the distance from the triangle to the line seg.
// This is complicated so we use the Mgc library.
static float CalcTriLineSegSquareDist(const Tri& tri, const LineSeg& line)
{
	MgcSegment3 mgcSeg;
	mgcSeg.Origin() = MgcVector3(line.GetStart().x(), line.GetStart().y(), line.GetStart().z());
	Vec3f dir = line.GetEnd() - line.GetStart();
	mgcSeg.Direction() = MgcVector3(dir.x(), dir.y(), dir.z());

	MgcTriangle3 mgcTri;
	mgcTri.Origin() = MgcVector3(tri.m_verts[0].x(), tri.m_verts[0].y(), tri.m_verts[0].z());
	Vec3f edge0 = tri.m_verts[1] - tri.m_verts[0];
	Vec3f edge1 = tri.m_verts[2] - tri.m_verts[0];
	mgcTri.Edge0() = MgcVector3(edge0.x(), edge0.y(), edge0.z());
	mgcTri.Edge1() = MgcVector3(edge1.x(), edge1.y(), edge1.z());

	return MgcSqrDistance(mgcSeg, mgcTri);
}


bool CollisionMesh::MultiIntersects(const Capsule& c, TriPtrs* pVec)
{
	float sqRadius = c.GetRadius() * c.GetRadius();
	for(Tris::iterator it = m_tris.begin(); it != m_tris.end(); ++it)
	{
		static int count = 0;

		float SqDist = CalcTriLineSegSquareDist(*it, c.GetLineSeg());

		if(SqDist < sqRadius)
		{
			pVec->push_back(&(*it));
		}
		count++;
	}
	return !pVec->empty();
}


bool CollisionMesh::Intersects(const Capsule& c, TriPtrs* pVec)
{
	std::pair<Tri*, float> TheBestTri;

	TheBestTri.second = 100000000;
	TheBestTri.first = nullptr;
	float sqRadius = c.GetRadius() * c.GetRadius();
	for(Tris::iterator it = m_tris.begin(); it != m_tris.end(); ++it)
	{

		static int count = 0;

		float SqDist = CalcTriLineSegSquareDist(*it, c.GetLineSeg());

		if(SqDist < sqRadius)
		{
			Plane TempPlane(it->m_verts[0], it->m_verts[1], it->m_verts[2]);
			if(TempPlane.ClassifyPoint(c.GetLineSeg().GetStart()) == 0)
			{
				if(SqDist < TheBestTri.second)
				{
					TheBestTri.first = &(*it);
					TheBestTri.second = SqDist;
				}
			}


			//pVec->push_back(&(*it));
		}
		count++;
	}

	if(TheBestTri.first != 0)
	{
		pVec->push_back(TheBestTri.first);
		return true;
	}

	return false;
}

void CollisionMesh::MakeMeshFromOBJ(OBJMesh* TheOBJ)
{
	Tri tri;

	int size = TheOBJ->ReturnFaces().size();
	Faces& Tempface = TheOBJ->ReturnFaces();

	for(int i = 0; i < size; i++)
	{
		for(int j = 0; j < 3; j++)
		{
			tri.m_verts[j] = TheOBJ->ReturnPositionVec()[Tempface[i].m_pointIndex[j] -1];
		}

		//std::swap(tri.m_verts[0], tri.m_verts[2]);

		Vec3f Normal(CrossProduct(tri.m_verts[0] - tri.m_verts[1], tri.m_verts[0] - tri.m_verts[2]));
		Normal.Normalise();
		
		tri.m_normal = Normal;

		
		AddTri(tri);
	}
}